//****************************************************************************
//
// drive.c.
//
//****************************************************************************

#include "inc/hw_types.h"
#include "drivers/motor.h"
#include "drivers/sensors.h"

#include "wireless.h"




static volatile unsigned long leftWheelTicks;
static volatile unsigned long rightWheelTicks;


static void DriveWheelSensorInterrupt(tWheel wheelSide)
{
	if(wheelSide == WHEEL_RIGHT)
		rightWheelTicks++;

	if(wheelSide == WHEEL_LEFT)
		leftWheelTicks++;
}

void DriveInit()
{
    /* Init the wheel tick counters */
    leftWheelTicks = 0;
    rightWheelTicks = 0;


    // Initialize the motor hardware
    MotorsInit();
    WheelSensorsInit(DriveWheelSensorInterrupt);


    // Enable the wheel sensors
    WheelSensorEnable();
    WheelSensorIntEnable(WHEEL_LEFT);
    WheelSensorIntEnable(WHEEL_RIGHT);
}

void DriveStop(void)
{
	MotorSpeed(RIGHT_SIDE, 0);
	MotorSpeed(LEFT_SIDE, 0);
	MotorStop(RIGHT_SIDE);
	MotorStop(LEFT_SIDE);
}

void DriveInstruction(DanceInstruction instruction)
{
	MotorDir(LEFT_SIDE, instruction.leftDirection);
	MotorDir(RIGHT_SIDE, instruction.rightDirection);
	MotorSpeed(LEFT_SIDE, instruction.leftWheelSpeed << 8);
	MotorSpeed(RIGHT_SIDE, instruction.rightWheelSpeed << 8);
	MotorRun(LEFT_SIDE);
	MotorRun(RIGHT_SIDE);
}


